/*
 * NAME, etc.
 *
 * sync.c
 *
 * Synchronization routines for SThread
 */
#define _REENTRANT

#include <sched.h>
#include "sthread.h"
#include "sync.h"

sthread_list_t* add_thread(sthread_list_t* p_head, sthread_t* p_sth) {
    sthread_list_t* head = p_head;

    if (p_head != NULL) {
		while (p_head->next != NULL)
			p_head = p_head->next;
		
		p_head->next   = (sthread_list_t*) malloc(sizeof(sthread_list_t));
		if (p_head->next == NULL) {
			printf("Malloc Error\n");
			return NULL;
		}
		p_head         = p_head->next;
		p_head->next   = NULL;
		p_head->thread = p_sth;
		
		return head;
    }

    else {
		p_head         = (sthread_list_t*) malloc(sizeof(sthread_list_t));
		p_head->next   = NULL;
		p_head->thread = p_sth;
		
		return p_head;
    }
}

sthread_list_t* pop_thread(sthread_list_t* p_head) {
    sthread_list_t* t_l;
    t_l = p_head->next;
    
	free(p_head);
    
	return t_l;	
}

int destroy_sthread_list(sthread_list_t* p_head) {
    while (p_head != NULL)
		p_head = pop_thread(p_head);

	return 0;
}

/*
 * Semaphore routines
 */

int sthread_sem_init(sthread_sem_t *sem, int count) {
	sem->count      = count;
	sem->n_waiting  = 0;
	sem->lock       = 0;
	sem->wait_queue = NULL;
	return 0;
}



int sthread_sem_destroy(sthread_sem_t *sem) {
	destroy_sthread_list(sem->wait_queue);
	free(sem);
	return 0;
}

int sthread_sem_down(sthread_sem_t *sem) {
        while (test_and_set(&sem->lock) != 0) { sched_yield(); };

	if (sem->count > 0) {
		--sem->count;
		sem->lock = 0;
		return 0;
	}
	else {
		sthread_t currentThread = sthread_self();
		sem->wait_queue = add_thread(sem->wait_queue, &currentThread);
		if(sem->wait_queue == NULL) {
			return -1;
		}
	
		++sem->n_waiting;
		sem->lock = 0;
		sthread_suspend();//suspend thread
		return 0;
	}

}
		   
int sthread_sem_try_down(sthread_sem_t *sem){
        while (test_and_set(&sem->lock) != 0) { sched_yield(); };//lock mutex
	
	if (sem->count > 0) {
		--sem->count;
		sem->lock = 0;
		return 0;
	}

	sem->lock = 0;//unlock mutex
	return -1;
}

int sthread_sem_up(sthread_sem_t *sem) {
	while (test_and_set(&sem->lock) != 0) { /* just spin */ };//lock mutex
			
	if (sem->n_waiting <= 0) {	
		++sem->count;
	}
	else {
		sthread_list_t* t_l;
		t_l = pop_thread(sem->wait_queue);
		sthread_wake(*(t_l->thread)); //wake up thread just popped
		free(t_l);

		--sem->n_waiting;	 
	}

	sem->lock = 0;//unlock mutex

	return 0;
}


